//////////////////////////////////////////////////////////////////////
// OctreeImpl.hpp
//
// WANG Yangzihao
// slashspirit@gmail.com
//
// Copyright (C) WANG Yangzihao, 2011, All rights reserved.
//////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////
//  Brief Description
//
//	Linear Octree Class implementation file
//
//////////////////////////////////////////////////////////////////////
namespace grflib { namespace engine { namespace aux {

    template<typename TObj, typename TRenderQueue>
    COctree<TObj, TRenderQueue>::COctree()
        : m_max_tree_depth(3), 
        m_rebuild_tree(true)
    {
    }

    template<typename TObj, typename TRenderQueue>
    COctree<TObj, TRenderQueue>::~COctree()
    {
    }

	// Add an object into octree
	// calculate its world bbox
    template<typename TObj, typename TRenderQueue>
    bool COctree<TObj, TRenderQueue>::AddObj(boost::shared_ptr<TObj> &pObj)
    {
        // Add scene obj into octree
        m_store.push_back(pObj);
		// not in render queue by default
		m_in_render_queue.push_back(false);
        // Calc bbox
        //using namespace core;
        //D3DXMATRIX worldMat = pObj->UpdateWorldMat();
        //pObj->UpdateBBox(worldMat);
        // set rebuild_tree
        m_rebuild_tree = true;
        return true;
    }

	// Remove an object from octree
    template<typename TObj, typename TRenderQueue>
    void COctree<TObj, TRenderQueue>::RemoveObj(boost::shared_ptr<TObj> &pObj)
    {
		std::vector<bool>::iterator iter2 = m_in_render_queue.begin();
        for (std::vector<boost::shared_ptr<TObj>>::iterator iter = m_store.begin();
            iter != m_store.end();
            ++iter, ++iter2)
        {
            if ((*iter) == pObj)
            {
				m_in_render_queue.erase(iter2);
                m_store.erase(iter);
            }
        }

        // set rebuild tree
        m_rebuild_tree = true;
    }

	// Clear the octree
    template<typename TObj, typename TRenderQueue>
    void COctree<TObj, TRenderQueue>::Clear(void)
    {
        m_store.resize(0);
        m_octree.clear();
        m_rebuild_tree = true;
    }

    template<typename TObj, typename TRenderQueue>
    void COctree<TObj, TRenderQueue>::NotifySceneUpdate()
    {
        m_rebuild_tree = true;
    }

    template<typename TObj, typename TRenderQueue>
    HRESULT COctree<TObj, TRenderQueue>::Traverse(const grflib::engine::core::IMemberCaller<TObj>& func)
    {
        HRESULT hr = S_OK;
        BOOST_FOREACH(boost::shared_ptr<TObj> &pObj, m_store)
        {
            V_RETURN( func(pObj) );
        }
        return hr;
    }

	// traverse the octree, set visibility for each node and each object
    template<typename TObj, typename TRenderQueue>
    void COctree<TObj, TRenderQueue>::NodeVisibilityTest(size_t index, const grflib::engine::core::CFrustum &frustum)
    {
		using namespace grflib::engine::core;
        BOOST_ASSERT(index < m_octree.size());

        octree_node_s& node = m_octree[index];

        const CFrustum::VIS vis = frustum.Visible(node.bb);
        node.vis = vis;
        if (CFrustum::VIS_PART == vis)
        {
            if ( node.first_child_index != -1 )
            {
                for ( int i = 0; i < 8; ++i )
                {
                    this->NodeVisibilityTest(node.first_child_index+i, frustum);
                }
            }
            else
            {
                BOOST_FOREACH(size_t idx, node.obj_indices)
                {
					if ( !m_in_render_queue[idx] )
					{
						boost::shared_ptr<CSceneObj> &pObj = m_store[idx];
						switch (frustum.Visible(pObj->GetWorldBBox()))
						{
						case CFrustum::VIS_PART:
						case CFrustum::VIS_YES:
							{
								m_in_render_queue[idx] = true;
								break;
							}
						}
					}
                }
            }
        }
        else
        {
            if (CFrustum::VIS_YES == vis)
            {
                BOOST_FOREACH(size_t idx, node.obj_indices)
                {
					if ( !m_in_render_queue[idx] )
					{
						m_in_render_queue[idx] = true;
					}
                }
            }
        }
    }

	// Rebuild the octree if needed (obj added, removed, or modified)
    template<typename TObj, typename TRenderQueue>
    void COctree<TObj, TRenderQueue>::Update(void)
    {
        using namespace grflib::engine::core;
        BBox<float> bb_root;
        if (m_rebuild_tree)
        {
            m_octree.resize(1);
            float min[3] = {0,0,0};
            bb_root.Clear();
            m_octree[0].first_child_index = -1;
            m_octree[0].obj_indices.clear();
            for ( size_t i = 0; i < m_store.size(); ++i )
            {
                // new bbox
                BBox<float> const & aabb_in_ws = m_store[i]->GetWorldBBox();
                bb_root.Merge(aabb_in_ws);
                m_octree[0].obj_indices.push_back(i);
            }
            
            m_octree[0].bb = bb_root;
            m_octree[0].depth = 0;

            m_base_address.push_back(0);
            m_base_address.push_back(1);

            for (uint32_t d = 1; d <= m_max_tree_depth; ++d )
            {
                size_t const original_size = m_octree.size();
                for ( size_t i = m_base_address[d-1]; i < m_base_address[d]; ++i )
                {
                    if (m_octree[i].obj_indices.size() > 1)
                    {
                        float parent_center[3];
                        float new_half_size[3];
                        for ( size_t k = 0; k < 3; ++k )
                        {
                            parent_center[k] = m_octree[i].bb.m_center[k];
                            new_half_size[k] = m_octree[i].bb.m_halfsize[k]/2.0f;
                        }
                        m_octree[i].first_child_index = static_cast<int>(m_base_address[d] + m_octree.size() - original_size);

                        for ( size_t j = 0; j < 8; ++j )
                        {
                            m_octree.push_back(octree_node_s());
                            octree_node_s& new_node = m_octree.back();
                            new_node.first_child_index = -1;
                            new_node.depth = d;
                            float bb_center[3];
                            if ( j&1 )
                            {
                                bb_center[0] = parent_center[0] + new_half_size[0];
                            }
                            else
                            {
                                bb_center[0] = parent_center[0] - new_half_size[0];
                            }
                            if ( j&2 )
                            {
                                bb_center[1] = parent_center[1] + new_half_size[1];
                            }
                            else
                            {
                                bb_center[1] = parent_center[1] - new_half_size[1];
                            }
                            if ( j&4 )
                            {
                                bb_center[2] = parent_center[2] + new_half_size[2];
                            }
                            else
                            {
                                bb_center[2] = parent_center[2] - new_half_size[2];
                            }
                            float temp_min[3] = {bb_center[0]-new_half_size[0],
                                bb_center[1]-new_half_size[1],
                                bb_center[2]-new_half_size[2]};
                            float temp_max[3] = {bb_center[0]+new_half_size[0],
                                bb_center[1]+new_half_size[1],
                                bb_center[2]+new_half_size[2]};
                            new_node.bb = BBox<float>(temp_min, temp_max);

                            BOOST_FOREACH(size_t obj_index, m_octree[i].obj_indices)
                            {
                                // new bbox
                                BBox<float> const & aabb_in_ws = m_store[obj_index]->GetWorldBBox();
                                if (((aabb_in_ws.m_min[0] <= new_node.bb.m_max[0]) && (aabb_in_ws.m_max[0] >= new_node.bb.m_min[0]))
                                    && ((aabb_in_ws.m_min[1] <= new_node.bb.m_max[1]) && (aabb_in_ws.m_max[1] >= new_node.bb.m_min[1]))
                                    && ((aabb_in_ws.m_min[2] <= new_node.bb.m_max[2]) && (aabb_in_ws.m_max[2] >= new_node.bb.m_min[2])))
                                {
                                    new_node.obj_indices.push_back(obj_index);
                                }
                            }

                        }
						
                    }
                }

                m_base_address.push_back(m_base_address.back() + m_octree.size() - original_size);
            }

            m_rebuild_tree = false;
        }

    }


	// Generate the renderqueue for a specific camera
    template<typename TObj, typename TRenderQueue>
    void COctree<TObj, TRenderQueue>::GenerateRenderQueue(TRenderQueue &renderQueue, 
                                                 const D3DXMATRIX *pViewMat,
                                                 const D3DXMATRIX *pProjMat)
    {
        using namespace grflib::engine::core;

        D3DMATRIX viewprojmat = (*pViewMat)*(*pProjMat);
        grflib::engine::core::CFrustum frustum;
        frustum.ClipMatrix(viewprojmat);
		NodeVisibilityTest(0, frustum);

		// Put visible objs into render queue

        //Sort objs in descending order
        struct CompFuncPtr
        {
            bool operator()( const boost::shared_ptr<CSceneObj> pObj1, const boost::shared_ptr<CSceneObj> pObj2 ) const
            {
                return pObj1->Priority() < pObj2->Priority();
            }
        };
        // if both physical visible (m_in_render_queue equals true)
        // logically visible (IsVisible() returns true) and is incullable(wow, what a word...)
        std::priority_queue<boost::shared_ptr<CSceneObj>,
                          std::vector<boost::shared_ptr<CSceneObj>>,
                          CompFuncPtr> priorityQueue;
		for ( size_t i = 0; i < m_in_render_queue.size(); ++i )
		{
			if ( (m_in_render_queue[i] || !m_store[i]->Cullable()) && m_store[i]->Visible() )
			{
                boost::shared_ptr<CSceneObj> &pObj = m_store[i];
                priorityQueue.push(pObj);
                m_in_render_queue[i] = false;
            }
		}

        D3DXMATRIX worldMat;
        //Append higher priority objs at the front of the render queue
        while (!priorityQueue.empty())
        {
            boost::shared_ptr<CSceneObj> &pObj = priorityQueue.top();
            worldMat = pObj->GetWorldMat();
            const std::vector< boost::shared_ptr<IRenderable> > &renderables = pObj->GetRenderables();
            BOOST_FOREACH(const boost::shared_ptr<IRenderable> &pRenderable, renderables)
            {
                renderQueue.push_back( std::make_pair(pRenderable, worldMat) );
            }
            priorityQueue.pop();
        }
    }

	// Get the first obj the given ray (with the origin at src and the direction of dir) hits in the scene
    template<typename TObj, typename TRenderQueue>
    boost::shared_ptr<TObj> COctree<TObj, TRenderQueue>::GetIntersectedObj(const float3& src, const float3& dir)
    {
        //BFS octree traverse
        std::queue<octree_node_s*> nodeQueue;
        boost::shared_ptr<TObj> pObj;
        float cur_dis = FLT_MAX;
        nodeQueue.push(&m_octree[0]);
        while ( !nodeQueue.empty() )
        {
            octree_node_s* node = nodeQueue.front();
            if ( RayBBoxIntersectTest(src, dir, node->bb) )
            {
                if ( node->first_child_index == -1 )
                {
                    //reach the max depth, start to test objs within this node
                    BOOST_FOREACH( size_t idx, node->obj_indices )
                    {
                        const grflib::engine::core::BBox<float> &box = m_store[idx]->GetWorldBBox();
                        if (pObj)
                        {	
                            if ( RayBBoxIntersectTest(src, dir, box) )
                            {
                                float dis = pow(box.m_center[0] - src._x, 2.0f)
                                    +pow(box.m_center[1] - src._y, 2.0f)
                                    +pow(box.m_center[2] - src._z, 2.0f);
                                if ( dis < cur_dis )
                                {
                                    pObj = m_store[idx];
                                    cur_dis = dis;
                                }
                            }
                        }
                        else
                        {
                            if ( RayBBoxIntersectTest(src, dir, m_store[idx]->GetWorldBBox() ) )
                            {
                                pObj = m_store[idx];
                                cur_dis = pow(box.m_center[0] - src._x, 2.0f)
                                    +pow(box.m_center[1] - src._y, 2.0f)
                                    +pow(box.m_center[2] - src._z, 2.0f);
                            }
                        }
                    }
                }
                else
                {
                    for ( size_t i = 0; i < 8; ++i )
                    {
                        nodeQueue.push(&m_octree[node->first_child_index+i]);
                    }
                }
            }
            nodeQueue.pop();
        }

        if ( pObj )
            return pObj;
        else
        {
            static boost::shared_ptr<TObj> empty;
            return empty;
        }
    }

    // ray-aabb intersection test based on:
    // "An Efficient and Robust Ray-Box Intersection Algorithm",
    // Amy Williams, Steve Barrus, R. Keith Morley, and Peter Shirley
    // Journal of graphics tools, 10(1):49-54, 2005
    template<typename TObj, typename TRenderQueue>
    bool COctree<TObj, TRenderQueue>::RayBBoxIntersectTest( const float3& src, const float3& dir, const grflib::engine::core::BBox<float>& box ) const
    {
        static const float t0 = -FLT_MAX;
        static const float t1 = FLT_MAX;

        float3 bbox[2] = {
            float3(box.m_min[0],box.m_min[1],box.m_min[2]),
            float3(box.m_max[0],box.m_max[1],box.m_max[2])
        };
        int xsign = (int)(dir._x < 0.0f);
        float invdirx = 1.0f / dir._x;
        float tmin = (bbox[xsign]._x - src._x) * invdirx;
        float tmax = (bbox[1-xsign]._x - src._x) * invdirx;

        int ysign = (int)(dir._y < 0.0f);
        float invdiry = 1.0f / dir._y;
        float tymin = (bbox[ysign]._y - src._y) * invdiry;
        float tymax = (bbox[1-ysign]._y - src._y) * invdiry;

        if ( (tmin > tymax) || (tymin > tmax) )
        {
            return false;
        }

        if (tymin > tmin) tmin = tymin;
        if (tymax < tmax) tmax = tymax;

        int zsign = (int)(dir._z < 0.0f);
        float invdirz = 1.0f / dir._z;
        float tzmin = (bbox[zsign]._z - src._z) * invdirz;
        float tzmax = (bbox[1-zsign]._z - src._z) * invdirz;

        if ( (tmin > tzmax) || (tzmin > tmax) )
        {
            return false;
        }

        if (tzmin > tmin) tmin = tzmin;
        if (tzmax < tmax) tmax = tzmax;

        bool res = (tmin < t1) && (tmax > t0);

        return res;
    }

}}} //namespaces